using System.Collections.Generic;
using UnityEngine;

namespace Gameplay.PVE
{
    public class SmoothModifier
    {
	    private List<List<Vector3>> listPool;
	    
	    public enum SmoothType
	    {
		    Simple,
		    Bezier,
		    OffsetSimple,
		    CurvedNonuniform,
	    }

	    public SmoothModifier()
	    {
		    listPool = new List<List<Vector3>>();
		    for (int i = 0; i < 5; i++)
		    {
			    listPool.Add(new List<Vector3>(200));
		    }
	    }

	    private List<Vector3> GetListFromPool()
	    {
		    if (listPool.Count == 0)
		    {
			    return new List<Vector3>();
		    }

		    var index = listPool.Count - 1;
		    var list = listPool[index];
		    listPool.RemoveAt(index);
		    return list;
	    }

	    private void RecoverListToPool(List<Vector3> list)
	    {
		    list.Clear();
		    listPool.Add(list);
	    }
	    
	    public SmoothType smoothType = SmoothType.Simple;

	    /** Number of times to subdivide when not using a uniform length */
	    [Tooltip("The number of times to subdivide (divide in half) the path segments. [0...inf] (recommended [1...10])")]
	    public int subdivisions = 2;

	    /** Number of times to apply smoothing */
	    [Tooltip("Number of times to apply smoothing")]
	    public int iterations = 2;

	    /** Determines how much smoothing to apply in each smooth iteration. 0.5 usually produces the nicest looking curves. */
	    [Tooltip("Determines how much smoothing to apply in each smooth iteration. 0.5 usually produces the nicest looking curves")]
	    [Range(0, 1)]
	    public float strength = 0.5F;

	    /** Toggle to divide all lines in equal length segments.
	     * \see #maxSegmentLength
	     */
	    [Tooltip("Toggle to divide all lines in equal length segments")]
	    public bool uniformLength = false;

	    /** The length of the segments in the smoothed path when using #uniformLength.
	     * A high value yields rough paths and low value yields very smooth paths, but is slower */
	    [Tooltip("The length of each segment in the smoothed path. A high value yields rough paths and low value yields very smooth paths, but is slower")]
	    public float maxSegmentLength = 2F;

	    /** Length factor of the bezier curves' tangents' */
	    [Tooltip("Length factor of the bezier curves' tangents")]
	    public float bezierTangentLength = 0.4F;

	    /** Offset to apply in each smoothing iteration when using Offset Simple. \see #smoothType */
	    [Tooltip("Offset to apply in each smoothing iteration when using Offset Simple")]
	    public float offset = 0.2F;

	    /** Roundness factor used for CurvedNonuniform */
	    [Tooltip("How much to smooth the path. A higher value will give a smoother path, but might take the character far off the optimal path.")]
	    public float factor = 0.2F;
	    
	    
        public List<Vector3> Smooth (List<Vector3> p) {
	        List<Vector3> path = null;

			switch (smoothType) {
			case SmoothType.Simple:
				SmoothSimple(ref p);
				path = p;
				break;
			case SmoothType.Bezier:
				path = SmoothBezier(p); break;
			case SmoothType.OffsetSimple:
				path = SmoothOffsetSimple(p); break;
			case SmoothType.CurvedNonuniform:
				path = CurvedNonuniform(p); break;
			}

			return path;
        }

		public List<Vector3> CurvedNonuniform (List<Vector3> path) {
			if (maxSegmentLength <= 0) {
				Debug.LogWarning("Max Segment Length is <= 0 which would cause DivByZero-exception or other nasty errors (avoid this)");
				return path;
			}

			int pointCounter = 0;
			for (int i = 0; i < path.Count-1; i++) {
				//pointCounter += Mathf.FloorToInt ((path[i]-path[i+1]).magnitude / maxSegmentLength)+1;

				float dist = (path[i]-path[i+1]).magnitude;
				//In order to avoid floating point errors as much as possible, and in lack of a better solution
				//loop through it EXACTLY as the other code further down will
				for (float t = 0; t <= dist; t += maxSegmentLength) {
					pointCounter++;
				}
			}

			List<Vector3> subdivided = new List<Vector3>(pointCounter);

			// Set first velocity
			Vector3 preEndVel = (path[1]-path[0]).normalized;

			for (int i = 0; i < path.Count-1; i++) {
				float dist = (path[i]-path[i+1]).magnitude;

				Vector3 startVel1 = preEndVel;
				Vector3 endVel1 = i < path.Count-2 ? ((path[i+2]-path[i+1]).normalized - (path[i]-path[i+1]).normalized).normalized : (path[i+1]-path[i]).normalized;

				Vector3 startVel = startVel1 * dist * factor;
				Vector3 endVel = endVel1 * dist * factor;

				Vector3 start = path[i];
				Vector3 end = path[i+1];

				float onedivdist = 1F / dist;

				for (float t = 0; t <= dist; t += maxSegmentLength) {
					float t2 = t * onedivdist;

					subdivided.Add(GetPointOnCubic(start, end, startVel, endVel, t2));
				}

				preEndVel = endVel1;
			}

			subdivided[subdivided.Count-1] = path[path.Count-1];

			return subdivided;
		}

		public static Vector3 GetPointOnCubic (Vector3 a, Vector3 b, Vector3 tan1, Vector3 tan2, float t) {
			float t2 = t*t, t3 = t2*t;

			float h1 =  2*t3 - 3*t2 + 1;          // calculate basis function 1
			float h2 = -2*t3 + 3*t2;              // calculate basis function 2
			float h3 =   t3 -  2*t2 + t;          // calculate basis function 3
			float h4 =   t3 -  t2;                // calculate basis function 4

			return h1*a +                            // multiply and sum all funtions
				   h2*b +                            // together to build the interpolated
				   h3*tan1 +                         // point along the curve.
				   h4*tan2;
		}

		public List<Vector3> SmoothOffsetSimple (List<Vector3> path) {
			if (path.Count <= 2 || iterations <= 0) {
				return path;
			}

			if (iterations > 12) {
				Debug.LogWarning("A very high iteration count was passed, won't let this one through");
				return path;
			}

			int maxLength = (path.Count-2)*(int)Mathf.Pow(2, iterations)+2;

			List<Vector3> subdivided =  new List<Vector3>(maxLength);
			List<Vector3> subdivided2 = new List<Vector3>(maxLength);

			for (int i = 0; i < maxLength; i++) { subdivided.Add(Vector3.zero); subdivided2.Add(Vector3.zero); }

			for (int i = 0; i < path.Count; i++) {
				subdivided[i] = path[i];
			}

			for (int iteration = 0; iteration < iterations; iteration++) {
				int currentPathLength = (path.Count-2)*(int)Mathf.Pow(2, iteration)+2;

				//Switch the arrays
				List<Vector3> tmp = subdivided;
				subdivided = subdivided2;
				subdivided2 = tmp;

				const float nextMultiplier = 1F;

				for (int i = 0; i < currentPathLength-1; i++) {
					Vector3 current = subdivided2[i];
					Vector3 next = subdivided2[i+1];

					Vector3 normal = Vector3.Cross(next-current, Vector3.up);
					normal = normal.normalized;

					bool firstRight = false;
					bool secondRight = false;
					bool setFirst = false;
					bool setSecond = false;
					if (i != 0 && !VectorMath.IsColinearXZ(current, next, subdivided2[i-1])) {
						setFirst = true;
						firstRight = VectorMath.RightOrColinearXZ(current, next, subdivided2[i-1]);
					}
					if (i < currentPathLength-1 && !VectorMath.IsColinearXZ(current, next, subdivided2[i+2])) {
						setSecond = true;
						secondRight = VectorMath.RightOrColinearXZ(current, next, subdivided2[i+2]);
					}

					if (setFirst) {
						subdivided[i*2] = current + (firstRight ? normal*offset*nextMultiplier : -normal*offset*nextMultiplier);
					} else {
						subdivided[i*2] = current;
					}

					if (setSecond) {
						subdivided[i*2+1] = next  + (secondRight ? normal*offset*nextMultiplier : -normal*offset*nextMultiplier);
					} else {
						subdivided[i*2+1] = next;
					}
				}

				subdivided[(path.Count-2)*(int)Mathf.Pow(2, iteration+1)+2-1] = subdivided2[currentPathLength-1];
			}

			//ListPool<Vector3>.Release(ref subdivided2);

			return subdivided;
		}

		public void SmoothSimple (ref List<Vector3> path) {
			if (path.Count < 2) return;

			List<Vector3> subdivided;

			if (uniformLength) {
				// Clamp to a small value to avoid the path being divided into a huge number of segments
				maxSegmentLength = Mathf.Max(maxSegmentLength, 0.005f);

				float pathLength = 0;
				for (int i = 0; i < path.Count-1; i++) {
					pathLength += Vector3.Distance(path[i], path[i+1]);
				}

				int estimatedNumberOfSegments = Mathf.FloorToInt(pathLength / maxSegmentLength);
				// Get a list with an initial capacity high enough so that we can add all points
				subdivided = GetListFromPool();//new List<Vector3>(estimatedNumberOfSegments+2);

				float distanceAlong = 0;

				// Sample points every [maxSegmentLength] world units along the path
				for (int i = 0; i < path.Count-1; i++) {
					var start = path[i];
					var end = path[i+1];

					float length = Vector3.Distance(start, end);

					while (distanceAlong < length) {
						subdivided.Add(Vector3.Lerp(start, end, distanceAlong / length));
						distanceAlong += maxSegmentLength;
					}

					distanceAlong -= length;
				}

				// Make sure we get the exact position of the last point
				subdivided.Add(path[path.Count-1]);
			} else {
				subdivisions = Mathf.Max(subdivisions, 0);

				if (subdivisions > 10) {
					Debug.LogWarning("Very large number of subdivisions. Cowardly refusing to subdivide every segment into more than " + (1 << subdivisions) + " subsegments");
					subdivisions = 10;
				}

				int steps = 1 << subdivisions;
				subdivided = GetListFromPool();//new List<Vector3>((path.Count-1)*steps + 1);
				Polygon.Subdivide(path, subdivided, steps);
			}

			if (strength > 0) {
				for (int it = 0; it < iterations; it++) {
					Vector3 prev = subdivided[0];

					for (int i = 1; i < subdivided.Count-1; i++) {
						Vector3 tmp = subdivided[i];

						// prev is at this point set to the value that subdivided[i-1] had before this loop started
						// Move the point closer to the average of the adjacent points
						subdivided[i] = Vector3.Lerp(tmp, (prev+subdivided[i+1])/2F, strength);

						prev = tmp;
					}
				}
			}
			path.Clear();
			path.AddRange(subdivided);
			RecoverListToPool(subdivided);
		}

		public List<Vector3> SmoothBezier (List<Vector3> path) {
			if (subdivisions < 0) subdivisions = 0;

			int subMult = 1 << subdivisions;
			List<Vector3> subdivided = new List<Vector3>();

			for (int i = 0; i < path.Count-1; i++) {
				Vector3 tangent1;
				Vector3 tangent2;
				if (i == 0) {
					tangent1 = path[i+1]-path[i];
				} else {
					tangent1 = path[i+1]-path[i-1];
				}

				if (i == path.Count-2) {
					tangent2 = path[i]-path[i+1];
				} else {
					tangent2 = path[i]-path[i+2];
				}

				tangent1 *= bezierTangentLength;
				tangent2 *= bezierTangentLength;

				Vector3 v1 = path[i];
				Vector3 v2 = v1+tangent1;
				Vector3 v4 = path[i+1];
				Vector3 v3 = v4+tangent2;

				for (int j = 0; j < subMult; j++) {
					subdivided.Add(AstarSplines.CubicBezier(v1, v2, v3, v4, (float)j/subMult));
				}
			}

			// Assign the last point
			subdivided.Add(path[path.Count-1]);

			return subdivided;
		}
    }
}